//
// Created by 29787 on 2023/12/26.
//
#include "UPRE_Decision.h"
#include "UPRE_ROBOT_TASK.h"

Motor_Set Motor_Set_param={
        .receive_ball           =null              ,
        .trans_ball             =null              ,
        .divide_ball            =null              ,
        .lay_ball               =null              ,
};

Percept_Pack Percept_RePack={
        .DivideBall.is_send_ball=false             ,
        .DivideBall.is_find_ball=false,
        .DivideBall.is_vent_ball=false             ,
        .DivideBall.is_lay_ball =false             ,
        .DivideBall.direction   =right_vent        ,
        .DivideBall.colour      =null_ball         ,
};

TimeStamp Timer_Stamp={
        .flag_is_protect_blueball=false,
        .flag_is_update_color=false,
        .flag_is_first_in_lay=true,
};

void Task_Decision() {
/////////////////////////////////////////Drain or eat balls according to color/////////////////////////////

    switch (Percept_RePack.DivideBall.colour) {
//        case null_ball: {                                            ///If the ball is not sent and not updated, the ball motor rotates
//            if (!Percept_RePack.DivideBall.is_send_ball) {           ///Not on the way to vent the ball
//                if (!Timer_Stamp.flag_is_update_color) {             ///Did not update the color of the ball
//                    Motor_Set_param.receive_ball = Vel_receive_ball;
//                }
//            }
//        }
//            break;

        case purplr_ball: {                                         ///Purple ball
            switch (Percept_RePack.DivideBall.direction) {
                case left_vent: {
                    Motor_Set_param.divide_ball = Vel_divide_ball;
                }
                    break;

                case right_vent: {
                    Motor_Set_param.divide_ball = -Vel_divide_ball;

                }
                    break;
            }
            Percept_RePack.DivideBall.is_vent_ball = true;         ///Set to vent ball status and the time stamp controls the motor rotation time
            Timer_Stamp.TimeStamp_is_vent_ball=null;
            Timer_Stamp.flag_is_update_color = null;
        }
            break;

        case blue_ball: {                                         ///Blue ball
            Percept_RePack.DivideBall.is_send_ball = true;        ///switch flag bit
            Percept_RePack.DivideBall.is_find_ball=false;
            Timer_Stamp.TimeStamp_is_protect_blueball=null;       ///Control the the time of motor spin to protect the blue ball after sending the ball
            Timer_Stamp.flag_is_protect_blueball=true;
        }
            break;
    }

    if (!Percept_RePack.DivideBall.is_vent_ball) {               ///The motor stalls isn‘t vent—ball state
        Motor_Set_param.divide_ball = null;
    }
/////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////lay ball/////////////////////////////////////////////////////////////
    if (Percept_RePack.DivideBall.is_lay_ball) {
        Timer_Stamp.flag_is_protect_blueball =false;             ///Clear the flag and timestamp of the protective ball
        Timer_Stamp.TimeStamp_is_protect_blueball=null;
        Motor_Set_param.lay_ball  = Vel_lay_ball;                ///The motor rotates and releases the ball and sets it to the ball eating state
        Motor_Set_param.trans_ball= Vel_trans_ball;
        Motor_Set_param.receive_ball=Vel_receive_ball;
    }
    else {
        Motor_Set_param.lay_ball = null;                         ///When not in volleyball, the speed is set to 0
    }
///////////////////////////////////////////////////////////////////////////////////////////////////

///////////control the time of vent ball to solve the situation of First purple and then blue//////
    if(Percept_RePack.DivideBall.is_vent_ball)                       ///the state of vent ball
    {
        if(Percept_RePack.DivideBall.colour==blue_ball)              ///if also eat blue ball
        {
            Timer_Stamp.TimeStamp_is_vent_ball=Timer_ventball-Timer_second_ball;   ///just let vent-ball-motor spin Timer_second_ball
        }
    }
///////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////Reach Zone 3 and start find ball///////////////////////////
    if(Chassis_Sense_Bool.Flag_ReachZone3)
    {
        static bool num=true;
        if(num) {
            Percept_RePack.DivideBall.is_find_ball=true;
            Motor_Set_param.receive_ball = Vel_receive_ball;
            Motor_Set_param.trans_ball   = Vel_trans_ball;
            num=false;
        }
    }
}

void Time_Stamp()
{
    Timer_Stamp.TimeStamp_is_vent_ball++;
    Timer_Stamp.TimeStamp_is_lay_ball++;
    Timer_Stamp.TimeStamp_is_update_color++;
    Timer_Stamp.TimeStamp_is_stop_receive_ball++;
    if(Timer_Stamp.TimeStamp_is_stop_receive_ball>=Timer_receiveball && Percept_RePack.DivideBall.colour==blue_ball)
    {
        Motor_Set_param.receive_ball=null;
    }

    if(Timer_Stamp.flag_is_protect_blueball)
        Timer_Stamp.TimeStamp_is_protect_blueball++;

    if(Timer_Stamp.TimeStamp_is_update_color>=Timer_updatecolor)     ///100msδ���յ������Ϣ����Ϊ����
    {
        Timer_Stamp.flag_is_update_color=false;
        Percept_RePack.DivideBall.colour = null_ball;
    }

    else{
        Timer_Stamp.flag_is_update_color=true;
    }

    if(Timer_Stamp.TimeStamp_is_vent_ball>=Timer_ventball)         ///����1500ms��ͣ��
    {
        Percept_RePack.DivideBall.is_vent_ball = false;
    }

    if(Timer_Stamp.TimeStamp_is_lay_ball>=Timer_layball)          ///���˷���㣬ת2000ms
    {
        Percept_RePack.DivideBall.is_lay_ball=false;
        if(!Percept_RePack.DivideBall.is_lay_ball && !Percept_RePack.DivideBall.is_send_ball)
            Percept_RePack.DivideBall.is_find_ball=true;
    }

    if(Timer_Stamp.TimeStamp_is_protect_blueball>=Timer_protect_blueball)
        Motor_Set_param.trans_ball=null;
}
